#include "System.h"

Incremental_pid  pid_speed1;
Incremental_pid  pid_speed2;
Incremental_pid  pid_speed3;
Incremental_pid  pid_speed4;

/**
  * @brief  PID参数初始化
	*	@note
  * @retval
  */
void PID_param_init()
{
    pid_speed1.err=0.0;
	pid_speed1.actual_val=0.0;
    pid_speed1.err_last=0.0;

	 pid_speed1.Kp = 10;
	 pid_speed1.Ki = 10;

    pid_speed2.err=0.0;
	pid_speed2.actual_val=0.0;
    pid_speed2.err_last=0.0;

	pid_speed2.Kp =10;
	pid_speed2.Ki =10 ;

    pid_speed3.err=0.0;
	pid_speed3.actual_val=0.0;
    pid_speed3.err_last=0.0;

	 pid_speed3.Kp = 10;
	 pid_speed3.Ki = 10;

    pid_speed4.err=0.0;
	pid_speed4.actual_val=0.0;
    pid_speed4.err_last=0.0;

	pid_speed4.Kp =10;
	pid_speed4.Ki =10 ;
#if 0
    float pid_temp1[2] = {pid_speed1.Kp, pid_speed1.Ki};
	float pid_temp2[2] = {pid_speed2.Kp, pid_speed2.Ki};
    float pid_temp3[2] = {pid_speed3.Kp, pid_speed3.Ki};
	float pid_temp4[2] = {pid_speed4.Kp, pid_speed4.Ki};

	set_computer_value(SEND_P_I_D_CMD, CURVES_CH1 ,pid_temp1,2);
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH2,pid_temp2,2);
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH3 ,pid_temp3,2);
	set_computer_value(SEND_P_I_D_CMD, CURVES_CH4,pid_temp4,2);

#endif
}


void set_p_i_d(Incremental_pid *pid, float p, float i, float d)
{
  	    pid->Kp = p;
		pid->Ki = i;
		pid->Kd = d;
}
/**
  * @brief  PID
  * @param  actual_val:ֵ
	*	@note
  * @retval
  */

float speed_pid_realize(Incremental_pid *pid,float Encoder,float Target)
{
		/*计算目标值与实际值的误差*/
    pid->err=Target-Encoder;

		/*PID算法实现*/
    pid->actual_val += pid->Kp*(pid->err-pid->err_last)+pid->Ki*pid->err;;

		/*误差传递*/
    pid->err_last=pid->err;
    
		/*返回当前实际值*/
    return pid->actual_val;
}
